 /*
 * PhotonRTOS础光实时操作系统 -- r核启动文件
 *
 * Copyright (C) 2022, 2023 国科础石(重庆)软件有限公司
 *
 * 作者: Xueming Dong <dongxueming@kernelsoft.com>
 *
 * License terms: GNU General Public License (GPL) version 3
 *
 */

#include <asm/asm-offsets.h>
#include <asm/stack.h>
/*#define CONFIG_DCLS 1*/

	.section	".head.text","ax"
	.align 2
	.globl reset
	.globl Reset_Handler
	.globl master_idle_stack

	.globl slave_cpu_entry

	.extern handle_arch_irq
	.extern handle_arch_fiq
	.extern undef_main
	.extern dabort_main
	.extern pabort_main
	.extern swi_main

	.extern handle_R5_irq

	.set initial_sp, master_idle_stack + THREAD_START_SP

	.equ    MODE_USR, 0x10
	.equ    MODE_FIQ, 0x11
	.equ    MODE_IRQ, 0x12
	.equ    MODE_SVC, 0x13
	.equ    MODE_ABT, 0x17
	.equ    MODE_UDF, 0x1b
	.equ    MODE_SYS, 0x1f

/**
 * 中断处理
 */
Reset_Handler:
	b       __test_main
undef:
	ldr     pc, =u_exception
swi:
	ldr     pc,=swi_handler
pabort:
	ldr     pc, =p_exception
dabort:
	ldr     pc, =d_exception
	ldr     pc, =d_exception
irq:
	ldr     pc, =irq_handler
fiq:
	ldr     pc, =fiq_handler

fiq_handler:
	sub     lr,lr,#4
	stmfd   sp!, {r0-r7, lr}
	ldr     lr,=fiq_ret
	ldr     pc,=handle_arch_fiq
fiq_ret:
	ldmia   sp!, {r0-r7, pc}^

irq_handler:
	sub     lr,lr,#4
	srsfd 	sp!, #0x1f @将当前模式下的lr和cpsr寄存器推入系统模式的栈中，此时在中断模式
	cps 	#0x1f @转换到系统模式

	push 	{r0-r3, r12} @根据AAPCS，主调者保存r0-r3, r12五个寄存器
	and 	r1, sp, #4 @将sp寄存器后两位清零

	sub	sp, sp, r1 @调用入口要保持8字节对齐
	push 	{r1, lr} @将r1和lr入栈，此时是在系统模式

	bl handle_R5_irq @调用handle

	pop 	{r1, lr} @恢复保存的现场
	add 	sp, sp, r1
	pop  	{r0-r3, r12}
	rfefd  	sp! @rfe，从异常返回命令，从当前模式的栈中恢复PC和CPSR

u_exception:
	sub     lr,lr,#4
	stmfd   sp!, {r0-r12, lr}
	ldr     lr,=undef_ret
	ldr     pc,=undef_main
undef_ret:
	ldmia   sp!, {r0-r12, pc}^

d_exception:
	sub     lr,lr,#8
	stmfd   sp!, {r0-r12, lr}
	mov     r0, lr
	mov     r1, sp
	ldr     lr,=dept_ret
	ldr     pc,=dabort_main
dept_ret:
	ldmia   sp!, {r0-r12, pc}^

p_exception:
	sub     lr,lr,#4
	stmfd   sp!, {r0-r12, lr}
	ldr     lr,=pept_ret
	ldr     pc,=pabort_main
pept_ret:
	ldmia   sp!, {r0-r12, pc}^


swi_handler:
	stmfd   sp!, {r0-r12, lr}
	ldr     lr,=swi_ret
	ldr     pc,=swi_main
swi_ret:
	ldmia   sp!, {r0-r12, pc}^

/**
 * 主程序开始
 */

	.globl __test_main
__test_main:


#if 0
	/**
	* 使能I-Cache
	*/
	mrc     p15, 0, r1, c1, c0, 0
	orr     r1, r1, #0x1 << 12
	dsb
	mcr     p15, 0, r1, c1, c0, 0
	isb
#endif

	/* enable cntvct */
	mrc p15, 0, r0, c9, c12, 0
	orr r0, r0, #0x1
	mcr p15, 0, r0, c9, c12, 0
	mov r0,#0x80000000
	mcr p15, 0, r0, c9, c12, 1

	/**
	* 禁止MPU和缓存
	*/
	mrc     p15, 0, r0, c1, c0, 0
	bic     r0, r0, #0x05
	bic     r0, r0, #0x1000
	dsb
	mcr     p15, 0, r0, c1, c0, 0
	isb

	/**
	* 禁止分支预测，TCM ECC checks
	*/
	mrc     p15, 0, r0, c1, c0, 1
	orr     r0, r0, #(0x1 << 17)
	orr     r0, r0, #(0x1 << 16)
	bic     r0, r0, #(0x1 << 15)
	orr     r0, r0, #(0x1 << 27)
	orr     r0, r0, #(0x1 << 26)
	orr     r0, r0, #(0x1 << 25)
	bic	    r0, r0, #(0x1 << 5)
	bic 	r0, r0, #(0x1 << 4)
	bic	    r0, r0, #(0x1 << 3)
	mcr     p15, 0, r0, c1, c0, 1
	dsb

	/**
	 * 设置中断向量表在0x00000000
	 */
	mrc	    p15, 0, r0,  c1,  c0, 0
	and     r0, r0, #0xffffdfff
	mcr     p15, 0, r0, c1, c0, 0

	/**
	 * 读取MPIDr寄存器
	 * 通过ID判断是哪个CPU，第二个CPU跳转到从CPU初始化
	 */
	mrc	    p15, 0, r0,  c0,  c0, 5
	ands	r0, r0, #0xFF
	bne	    slave_cpu_entry

	/*
	 * 配置各种模式的栈
	 */

	//irq
	mrs     r2,cpsr
	bic     r2,#0X1f
	orr     r2,#MODE_IRQ
	msr     cpsr,r2
	ldr     sp,=__IrqStack
	//mov   lr,0

	//fiq
	mrs     r2,cpsr
	bic     r2,#0X1f
	orr     r2,#MODE_FIQ
	msr     cpsr,r2
	ldr     sp,=__FiqStack
	//mov   lr,0

	//svc
	mrs     r2,cpsr
	bic     r2,#0X1f
	orr     r2,#MODE_SVC
	msr     cpsr,r2
	ldr     sp,=__SwiStack
	//mov   lr,0

	//abort
	mrs  r2,cpsr
	bic  r2,#0X1f
	orr  r2,#MODE_ABT
	msr  cpsr,r2
	ldr  sp,=__AbdStack
	//mov  lr,0

	//undef
	mrs     r2,cpsr
	bic     r2,#0X1f
	orr     r2,#MODE_UDF
	msr     cpsr,r2
	ldr     sp,=__UndStack
	//mov   lr,0

	//sys
	mrs     r2,cpsr
	bic     r2,#0X1f
	orr     r2,#MODE_SYS
	msr     cpsr,r2
	ldr     sp,=initial_sp
	//mov   lr,0

	mov     r1,#0

#if 0
	/**
	 * 判断是否需要搬移代码和数据
	 * 目前没有使用
	 */
	b       loop_relocal_code

relocal_code:
	ldr     r3, =__reloce_src
	ldr     r3, [r3, r1]
	str     r3, [r0, r1]
	adds    r1, r1, #4

loop_relocal_code:
	ldr     r0, =__reloce_start
	ldr     r3, =__reloce_end
	adds    r2, r0, r1
	cmp     r2, r3
	bcc     relocal_code
	mov     r1,#0
	b       LoopCopyDataInit

CopyDataInit:
	ldr     r3, =_sidata
	ldr     r3, [r3, r1]
	str     r3, [r0, r1]
	adds    r1, r1, #4

LoopCopyDataInit:
	ldr     r0, =_sdata
	ldr     r3, =_edata
	adds    r2, r0, r1
	cmp     r2, r3
	bcc     CopyDataInit

#endif

	/**
	* bss数据段清零
	*/
	ldr     r2, =__bss_start
	b       LoopFillZerobss

FillZerobs:
	movs    r3, #0
	str     r3, [r2], #4

LoopFillZerobss:
	ldr     r3, = __bss_stop
	cmp     r2, r3
	bcc     FillZerobs

/**
 * 初始化堆栈
 */
 .globl __user_initial_stackheap

__user_initial_stackheap:
	mov     r0,#0
	mov     r1,#0
	mov     r2,#0
	mov     r3,#0

	mov     r4,#0
	mov     r5,#0
	mov     r6,#0
	mov     r7,#0
	mov     r8,#0

	mov     r9,#0
	mov     r10,#0
	mov     r11,#0
	mov     r12,#0
	mov     r14,#0
	ldr     sp, =initial_sp // Heap base

	.extern arch_cpu_init

	.extern start_master

	/**
	 * 跳转到start_master，进行启动
	 */
	ldr     pc ,=start_master
	/**
	 * 不会运行到这里
	 */
	b       __test_main

/**
 * 从CPU启动
 */
slave_cpu_entry:
	.extern slave_cpu_data
	.extern start_slave

	/*
	 * 设置各种模式的栈空间
	 */
	//irq
	mrs     r2,cpsr
	bic     r2,#0X1f
	orr     r2,#MODE_IRQ
	msr     cpsr,r2
	ldr     sp,=__IrqStack1
	//mov   lr,0

	//fiq
	mrs     r2,cpsr
	bic     r2,#0X1f
	orr     r2,#MODE_FIQ
	msr     cpsr,r2
	ldr     sp,=__FiqStack1
	//mov   lr,0

	//svc
	mrs     r2,cpsr
	bic     r2,#0X1f
	orr     r2,#MODE_SVC
	msr     cpsr,r2
	ldr     sp,=__SwiStack1
	//mov   lr,0

	//abort
	mrs     r2,cpsr
	bic     r2,#0X1f
	orr     r2,#MODE_ABT
	msr     cpsr,r2
	ldr     sp,=__AbdStack1
	//mov   lr,0

	//undef
	mrs     r2,cpsr
	bic     r2,#0X1f
	orr     r2,#MODE_UDF
	msr     cpsr,r2
	ldr     sp,=__UndStack1
	//mov   lr,0

	//sys
	mrs     r2,cpsr
	bic     r2,#0X1f
	orr     r2,#MODE_SYS
	msr     cpsr,r2
	ldr     r0, =slave_cpu_data
	ldr     sp, [r0]

	// Init regs 0-3
	mov     r0,#0
	mov     r1,#0
	mov     r2,#0
	mov     r3,#0

	// Init regs 4-8
	mov     r4,#0
	mov     r5,#0
	mov     r6,#0
	mov     r7,#0
	mov     r8,#0

	// Init regs 9-14
	mov     r9,#0
	mov     r10,#0
	mov     r11,#0
	mov     r12,#0
	mov     r14,#0

	ldr     r0, =slave_cpu_data
	ldr     sp, [r0]

	/**
	 * 从CPU启动
	 */
	ldr     PC,=start_slave
	/**
	 * 不会运行到这里
	 */
	b       __test_main

.end
